
#include "gsl_ext_multimin.h"
#include "lrwwsimplex_state.h"

#include <gsl/gsl_blas.h>
#include <gsl/gsl_sf.h>

static const double gamma_ = 0.5;
static const double chi    = 2.0;
static const double rho    = 1.0;
static const double sigma  = 0.5;

static simplex *simplex_alloc(int n)
{
  int i;
	
  simplex *S = (simplex *)malloc(sizeof(simplex));
	
  S->n = n + 1;
  S->v = (gsl_vector **)malloc((n + 1) * sizeof(gsl_vector *));
  for(i = 0; i < n + 1; i++)
    S->v[i] = gsl_vector_alloc(n);
  S->fv = gsl_vector_alloc(n + 1);
	
  /* Compute initial simplex volume from */
  /* 1/n!*|det(v1,...,vn)|*product(||vi||), where i=1,...,n and */
  /* vectors v1,...,vn are edge vectors. */
  /* In this case, the initial edge matrix is identity matrix, */
  /* so det(v1,...,vn)=1 and product(||vi||)=1. */
  S->V = 1.0 / gsl_sf_fact(n);
	
  return S;
}

static void simplex_compute_centroid(const simplex *S, gsl_vector *xc)
{
  int i;
	
  gsl_vector_set_zero(xc);
	
  for(i = 0; i < S->n - 1; i++)
    gsl_blas_daxpy(1.0, S->v[i], xc);
  
  gsl_blas_dscal(1.0 / (S->n - 1), xc);
}

static void simplex_free(simplex *S)
{
  int i;
  
  gsl_vector_free(S->fv);
  for(i = 0; i < S->n; i++)
    gsl_vector_free(S->v[i]);
  free(S->v);
  free(S);
}

static void simplex_inside_contraction_point(const simplex *S, 
                                             const gsl_vector *xc, 
                                             gsl_vector *xic)
{
  /* xic = (1.0 - gamma) * xc + gamma*vertices.back().x; */
  gsl_blas_dcopy(xc, xic);
  gsl_blas_dscal(1.0 - gamma_, xic);
  gsl_blas_daxpy(gamma_, S->v[S->n - 1], xic);
}

static void simplex_outside_contraction_point(const simplex *S, 
                                              const gsl_vector *xc, 
                                              gsl_vector *xoc)
{
  /* xoc = (1.0 + rho*gamma) * xc - rho*gamma*vertices.back().x; */
  gsl_blas_dcopy(xc, xoc);
  gsl_blas_dscal(1.0 + rho*gamma_, xoc);
  gsl_blas_daxpy(-rho * gamma_, S->v[S->n - 1], xoc);
}

static void simplex_expansion_point(const simplex *S, 
                                    const gsl_vector *xc, 
                                    gsl_vector *xe)
{
  /* xe = (1.0 + rho*khi) * xc - rho*chi*vertices.back().x; */
  gsl_blas_dcopy(xc, xe);
  gsl_blas_dscal(1.0 + rho*chi, xe);
  gsl_blas_daxpy(-rho*chi, S->v[S->n - 1], xe);
}

static void simplex_reflection_point(const simplex *S, 
                                     const gsl_vector *xc, 
                                     gsl_vector *xr)
{
  /* xr = (1.0 + rho) * xc - rho*vertices.back().x; */
  gsl_blas_dcopy(xc, xr);
  gsl_blas_dscal(1.0 + rho, xr);
  gsl_blas_daxpy(-rho, S->v[S->n - 1], xr);
}

static void simplex_shrink(simplex *S, 
                           const gsl_multimin_function *f)
{
  int i;
	
  for(i = 1; i < S->n; i++)
  {
    /* v[i] = v[i_lo] + sigma * (vertices[i] - v[i_lo]); */
    gsl_blas_dscal(sigma, S->v[i]);
    gsl_blas_daxpy(1.0 - sigma, S->v[0], S->v[i]);
    
    gsl_vector_set(S->fv, i, GSL_MULTIMIN_FN_EVAL_F(f, S->v[i]));
    
    S->V *= sigma;
  }
}

static void simplex_set_vertex(simplex *S, int i, const gsl_vector *x, double fx)
{
  gsl_blas_dcopy(x, S->v[i]);
  gsl_vector_set(S->fv, i, fx);
}

static void simplex_search_direction(gsl_vector *xc, gsl_vector *xr, gsl_vector *d)
{
  gsl_blas_dcopy(xc, d);
  gsl_blas_daxpy(-1.0, xr, d);
}

static void simplex_improve_highest_vertex(simplex *S, const gsl_vector *v, double vf)
{
  int i = S->n - 2;
  
  while(i >= 0 && vf < gsl_vector_get(S->fv, i))
  {
    gsl_vector_set(S->fv, i + 1, gsl_vector_get(S->fv, i));
    gsl_blas_dcopy(S->v[i], S->v[i + 1]);
    i--;
  }
  
  gsl_vector_set(S->fv, i + 1, vf);
  gsl_blas_dcopy(v, S->v[i + 1]);
}

static void simplex_sort_vertices(simplex *S)
{
  int i, j;
  double fv;
  gsl_vector *tmpv = gsl_vector_alloc(S->n - 1);
	
  for(i = 1; i < S->n; i++)
  {
    fv = gsl_vector_get(S->fv, i);
    gsl_blas_dcopy(S->v[i], tmpv);
    j = i - 1;
  
    while(j >= 0 && gsl_vector_get(S->fv, j) > fv)
    {
      gsl_vector_set(S->fv, j + 1, gsl_vector_get(S->fv, j));
      gsl_blas_dcopy(S->v[j], S->v[j + 1]);
      j--;
    }
  
    gsl_vector_set(S->fv, j + 1, fv);
    gsl_blas_dcopy(tmpv, S->v[j + 1]);
  }
	
  gsl_vector_free(tmpv);
}

static int lrwwsimplex_alloc(void *vstate, size_t n)
{
  lrwwsimplex_state_t *state = (lrwwsimplex_state_t *)vstate;
	
  state->S = simplex_alloc(n);
  state->d = gsl_vector_alloc(n);
  state->xc = gsl_vector_alloc(n);
  state->xr = gsl_vector_alloc(n);
  state->xe = gsl_vector_alloc(n);
  state->xcc = gsl_vector_alloc(n);
	
  return GSL_SUCCESS;
}

static int lrwwsimplex_set(void *vstate, gsl_multimin_function *f,
                           const gsl_vector *x,
                           double *size,
                           const gsl_vector *step_size)
{
  const int N = f->n + 1;
	
  int i;
  double fx;
  double fxi;
	
  lrwwsimplex_state_t *state = (lrwwsimplex_state_t *)vstate;
	
  gsl_vector *xi = gsl_vector_alloc(N - 1);

  fx = GSL_MULTIMIN_FN_EVAL_F(f, x);
  simplex_set_vertex(state->S, 0, x, fx);
  for(i = 1; i < N; i++)
    {
      gsl_blas_dcopy(x, xi);
      gsl_vector_set(xi, i - 1, gsl_vector_get(xi, i - 1) + gsl_vector_get(step_size, i - 1));
      gsl_blas_dcopy(xi, state->S->v[i]);
      fxi = GSL_MULTIMIN_FN_EVAL_F(f, xi);
      gsl_vector_set(state->S->fv, i, fxi);
    }
	
  gsl_vector_free(xi);
	
  simplex_sort_vertices(state->S);
	
  return GSL_SUCCESS;
}

static int lrwwsimplex_iterate(void *vstate, gsl_multimin_function *f,
                               gsl_vector *x, 
                               double *size, 
                               double *fval)
{
  lrwwsimplex_state_t *state = (lrwwsimplex_state_t *)vstate;
	
  const int N = f->n;
	
  double fr;
  double fe;
  double fcc;
	
  /* Step 1.:compute the centroid point */
  simplex_compute_centroid(state->S, state->xc);
	
  /* Step 2.:reflection */
  simplex_reflection_point(state->S, state->xc, state->xr);
  fr = GSL_MULTIMIN_FN_EVAL_F(f, state->xr);
  simplex_search_direction(state->xc, state->xr, state->d);
  if(gsl_vector_get(state->S->fv, 0) <= fr && fr < gsl_vector_get(state->S->fv, N - 1))
    {
      simplex_set_vertex(state->S, N, state->xr, fr);
      simplex_improve_highest_vertex(state->S, state->xr, fr);
      state->S->V *= rho;
      goto end;
    }
	
  /* Step 3.:expansion */
  if(fr < gsl_vector_get(state->S->fv, 0))
  {
    simplex_expansion_point(state->S, state->xc, state->xe);
    fe = GSL_MULTIMIN_FN_EVAL_F(f, state->xe);
  
    if(fe < fr)
    {
      simplex_improve_highest_vertex(state->S, state->xe, fe);
      state->S->V *= rho * chi;
    }
    else
    {
      simplex_improve_highest_vertex(state->S, state->xr, fr);
      state->S->V *= rho;
    }
  
    goto end;
  }
	
  /* Step 4.:contraction */
  if(fr >= gsl_vector_get(state->S->fv, N - 1))
  {
    /* case 1.: outside */
    if(gsl_vector_get(state->S->fv, N - 1) <= fr && fr < gsl_vector_get(state->S->fv, N))
    {
      simplex_outside_contraction_point(state->S, state->xc, state->xcc);
      fcc = GSL_MULTIMIN_FN_EVAL_F(f, state->xcc);
      
      if(fcc <= fr)
      {
	simplex_set_vertex(state->S, N, state->xcc, fcc);
	simplex_improve_highest_vertex(state->S, state->xcc, fcc);
	state->S->V *= rho * gamma_;
	goto end;
      }
    }
    /* case 2.: inside */
    else if(fr >= gsl_vector_get(state->S->fv, N))
    {
      simplex_inside_contraction_point(state->S, state->xc, state->xcc);
      fcc = GSL_MULTIMIN_FN_EVAL_F(f, state->xcc);
  
      if(fcc < gsl_vector_get(state->S->fv, N))
      {
	simplex_set_vertex(state->S, N, state->xcc, fcc);
	simplex_improve_highest_vertex(state->S, state->xcc, fcc);
	state->S->V *= gamma_;
	goto end;
      }
    }
  }
	
  /* Step 5.:shrink if the previous steps did not yield any improvement */
  simplex_shrink(state->S, f);
  simplex_sort_vertices(state->S);
  
  end:
  gsl_blas_dcopy(state->S->v[0], x);
  *fval = gsl_vector_get(state->S->fv, 0);
  *size = state->S->V;
	
  return GSL_SUCCESS;
}

static void lrwwsimplex_free(void *vstate)
{
  lrwwsimplex_state_t *state = (lrwwsimplex_state_t *)vstate;
  
  gsl_vector_free(state->xe);
  gsl_vector_free(state->xr);
  gsl_vector_free(state->xc);
  gsl_vector_free(state->xcc);
  gsl_vector_free(state->d);
  simplex_free(state->S);
}

static const gsl_multimin_fminimizer_type lrwwsimplex_type = 
{
  "lrwwsimplex",
  sizeof (lrwwsimplex_state_t),
  &lrwwsimplex_alloc,
  &lrwwsimplex_set,
  &lrwwsimplex_iterate,
  &lrwwsimplex_free
};

const gsl_multimin_fminimizer_type
* gsl_ext_multimin_fminimizer_lrwwsimplex = &lrwwsimplex_type;
